#include <system.h>

enum xpacket_enums {
	
	XPACKET_MAX_BUFF = 8,
};


struct {
	
	uint8_t len;
	message_t* msg;
	
} xpacket_txQ[XPACKET_MAX_BUFF];


BOOL xpacket_txFull, xpacket_txBusy;
uint8_t xpacket_txIn, xpacket_txOut;

task_t xpacket_taskTx;

message_t* xpacket_txmsg;


void XPACKET_TASKTX_run(void);


void XPACKET_init(void) {
	
	xpacket_txBusy = xpacket_txFull = FALSE;
	xpacket_txIn = xpacket_txOut = 0;
	
	TASK_reg(&xpacket_taskTx, XPACKET_TASKTX_run);
}


error_t XPACKET_send(uint8_t type, message_t* msg, uint8_t len) {
	
	if (len > XPACKET_LENGTH)
		return ESIZE;
	
	if (xpacket_txFull)
		return EBUSY;
	
	XPACKET_setType(msg, type);
	XPACKET_setSeq(msg, 0);
	
	xpacket_txQ[xpacket_txIn].len = len;
	xpacket_txQ[xpacket_txIn].msg = msg;
	
	xpacket_txIn = (xpacket_txIn + 1) & XPACKET_MAX_BUFF;
	
	if (xpacket_txIn == xpacket_txOut)
		xpacket_txFull = TRUE;
	
	if (!xpacket_txBusy) {
		
		xpacket_txBusy = TRUE;
		TASK_post(&xpacket_taskTx);
	}
	
	return SUCCESS;
}


void XPACKET_sendDone(error_t err) {
	
	xpacket_txOut = (xpacket_txOut + 1) % XPACKET_MAX_BUFF;
	xpacket_txFull = FALSE;
	
	uint8_t type = XPACKET_getType(xpacket_txmsg);
	
	switch (type) {
		
	case XPACKET_TYPE_MOTOR:
		
		break;
		
	case XPACKET_TYPE_PING:
		
		XUTIL_pingSendDone(err);
		break;
		
	case XPACKET_TYPE_CAMERA:
		
		XCAMERA_senddone(err);
		break;
	}
	
	TASK_post(&xpacket_taskTx);
}


void XPACKET_TASKTX_run(void) {
	
	if (!xpacket_txFull && xpacket_txIn == xpacket_txOut) {
		
		xpacket_txBusy = FALSE;
		return;
	}
	
	uint8_t len = xpacket_txQ[xpacket_txOut].len;
	message_t* msg = xpacket_txQ[xpacket_txOut].msg;
	
	xpacket_txmsg = msg;
	
	error_t err = CXBEE_send(5, msg, len + 2);
	
	if (err != SUCCESS) {
		XPACKET_sendDone(err);
	}
}


message_t*  XPAKCET_receive(message_t* msg, void* payload, uint8_t len) {
	
	switch(XPACKET_getType(msg)) {
		
	case XPACKET_TYPE_MOTOR:
		
		msg = XUTIL_motorReceive(msg);
		break;
		
	case XPACKET_TYPE_PING:
		
		msg = XUTIL_pingReceive(msg);
		break;
		
	case XPACKET_TYPE_CAMERA:
		
		msg = XCAMERA_receive(msg);
		break;
	}
	
	return msg;
}


/******************************************************************************
 *                           PACKEY API
 */

void XPACKET_setType(message_t* msg, uint8_t type) {
	
	xpacket_t* pkt = (xpacket_t*) CXBEE_getPayload(msg);
	pkt->type = type;
}

uint8_t XPACKET_getType(message_t* msg) {
	
	xpacket_t* pkt = (xpacket_t*) CXBEE_getPayload(msg);
	return pkt->type;
}

/*void XPACKET_setPort(message_t* msg, uint8_t port) {
	
	xpacket_t* pkt = (xpacket_t*) CXBEE_getPayload(msg);
	pkt->port = port;
}

uint8_t XPACKET_getPort(message_t* msg) {
	
	xpacket_t* pkt = (xpacket_t*) CXBEE_getPayload(msg);
	return pkt->port;
}*/

void XPACKET_setSeq(message_t* msg, uint8_t seq) {
	
	xpacket_t* pkt = (xpacket_t*) CXBEE_getPayload(msg);
	pkt->seq = seq;
}

uint8_t XPACKET_getSeq(message_t* msg) {
	
	xpacket_t* pkt = (xpacket_t*) CXBEE_getPayload(msg);
	return pkt->seq;
}

void* XPACKET_getData(message_t* msg) {
	
	xpacket_t* pkt = (xpacket_t*) CXBEE_getPayload(msg);
	return pkt->data;
}